#include "include.h"



BALANCE_CONTROL_t balance_control;
VISION_CONTROL_t vision_control;
MOTOR_CONTROL_t motor_control;
char buff[20];
int32 Pulse1 = 0,Pulse2 = 0;
int32 delayTime;
int16 forward_vel = 0,balance_vel=0,turn_vel = 0;
int16 left=0;
int16 right=0;
int32 ccd_sum=0;

void motor_out();
void motor_display();
void balance_out();
void vision_out();
void solution();

void balance_display();
void motor_display();

void HardWareInit()
{
   uart_init(UART0, 1500000);
   OLED_Init();
   OLED_CLS();
   I2C_Init();
   CCD_Init();
   FTM_QUAD_Init(FTM1);
   FTM_QUAD_Init(FTM2);
   FTM_PWM_init(FTM0, FTM_CH0, 10 * 1000, 0);
   FTM_PWM_init(FTM0, FTM_CH1, 10 * 1000, 0);
   FTM_PWM_init(FTM0, FTM_CH2, 10 * 1000, 0);
   FTM_PWM_init(FTM0, FTM_CH3, 10 * 1000, 0);
}

void InterruptInit()
{
   pit_init_ms(PIT0, INTERRUPT0_BALANCE_CONTROL_TIME);                              
   set_vector_handler(PIT0_VECTORn, balance_out);
   EnableInterrupts;
   enable_irq(PIT0_IRQn);
   
   pit_init_ms(PIT1, INTERRUPT1_MOTOR_CONTROL_TIME);
   set_vector_handler(PIT1_VECTORn, motor_out);
   EnableInterrupts;
   enable_irq(PIT1_IRQn);
/*
   pit_init_ms(PIT2, INTERRUPT2_VISION_CONTROL_TIME);
   set_vector_handler(PIT2_VECTORn, vision_out);
   EnableInterrupts;
   enable_irq(PIT2_IRQn);*/

}

void main(void)
{
   HardWareInit();
   BALANCE_CONTROL_StructInit(&balance_control);
   VISION_CONTROL_StructInit(&vision_control);
   MOTOR_CONTROL_StructInit(&motor_control);
   balance_control.f_init_balance_control(&balance_control);
   vision_control.f_init_vision_control(&vision_control);
   motor_control.f_init_motor_control(&motor_control);
   
   InterruptInit();
   
   while (1)
   {
      balance_display();
      //motor_display();
      //WIFI����test close-loop vel control
      printf("%d/r/n",motor_control.left_current_vel);
      printf("%d/r/n/r/n",motor_control.left_target_vel);
      DELAY_MS(10);
   }
}

void balance_display()
{
   sprintf(buff,"%d",(left));
   OLED_P6x8Str(20,3,"turn_v=");
   OLED_P6x8Str(20+56,3,buff); 
   OLED_P6x8Char(' ');        
   sprintf(buff,"%d",(int)(balance_control.balance_control_current_angle + balance_control.Gyro_Y*INTERRUPT0_BALANCE_CONTROL_TIME/1000));
   OLED_P6x8Str(20,4,"Gyro_Y=");
   OLED_P6x8Str(20+56,4,buff); 
   OLED_P6x8Char(' ');
   sprintf(buff,"%d",(int)(balance_control.balance_control_current_angle));
   OLED_P6x8Str(20,5,"C_Ang"); 
   OLED_P6x8Str(20+56,5,buff); 
   OLED_P6x8Char(' ');
   sprintf(buff,"%d",(int)(motor_control.left_motor_output));
   OLED_P6x8Str(20,6,"fad_out=");
   OLED_P6x8Str(20+56,6,buff); 
   OLED_P6x8Char(' ');
}

void balance_out()
{
    PIT_Flag_Clear(PIT0);
    balance_control.f_update_attitude(&balance_control);
    //refer to https://blog.csdn.net/qq_49979053/article/details/117395838
    //attitude solution with complementary filtering
    //Suppose the z-axis is vertical to angle zero
    balance_control.balance_control_current_angle = (balance_control.balance_control_current_angle + balance_control.Gyro_Y*INTERRUPT0_BALANCE_CONTROL_TIME/1000.0);
    balance_control.balance_control_pid.f_Calculate(&(balance_control.balance_control_pid),balance_control.balance_control_current_angle,balance_control.balance_control_target_angle);
    balance_control.balance_control_output = balance_control.balance_control_pid.output;
}

void motor_display()
{
   sprintf(buff,"%d",(int)(motor_control.left_motor_output));
   OLED_P6x8Str(20,3,"L_out=");
   OLED_P6x8Str(20+56,3,buff);
   OLED_P6x8Char(' ');        
   sprintf(buff,"%d",(int)(motor_control.right_motor_output));
   OLED_P6x8Str(20,4,"R_out=");
   OLED_P6x8Str(20+56,4,buff); 
   OLED_P6x8Char(' ');
   sprintf(buff,"%d",motor_control.left_current_vel);
   OLED_P6x8Str(20,5,"L_vel="); 
   OLED_P6x8Str(20+56,5,buff); 
   OLED_P6x8Char(' ');
   sprintf(buff,"%d",motor_control.right_current_vel);
   OLED_P6x8Str(20,6,"R_vel=");
   OLED_P6x8Str(20+56,6,buff); 
   OLED_P6x8Char(' ');
}

void solution()
{
  balance_vel = (int)(balance_control.balance_control_output);
  forward_vel = (int)(motor_control.forward_output);
  turn_vel = 0;
  motor_control.left_motor_output = balance_vel + forward_vel+turn_vel;
  motor_control.right_motor_output = balance_vel+ forward_vel - turn_vel;
}

void motor_out()
{       
   PIT_Flag_Clear(PIT1);
   Pulse1=FTM_QUAD_get(FTM2);  
   Pulse2=-FTM_QUAD_get(FTM1);  
   
   motor_control.left_current_vel = Pulse1 - motor_control.left_motor_count;
   motor_control.right_current_vel = Pulse2 - motor_control.right_motor_count;
   motor_control.left_motor_count = Pulse1;
   motor_control.right_motor_count = Pulse2;
   motor_control.forward_current_vel = (motor_control.left_current_vel + motor_control.right_current_vel)/2.0;
   motor_control.forward_pid.f_Calculate(&(motor_control.forward_pid),motor_control.forward_current_vel,motor_control.forward_target_vel);
   motor_control.forward_output = motor_control.forward_pid.output;
   solution();
   /*
   
   
   //set target_angle
   //balance_control.balance_control_target_angle = motor_control.forward_output + BALANCE_CONTROL_TARGET_ANGLE;
   
   
   //PID
   motor_control.left_motor_control_pid.f_Calculate(&(motor_control.left_motor_control_pid),motor_control.left_current_vel,motor_control.left_target_vel);
   motor_control.left_motor_output = motor_control.left_motor_control_pid.output;
   motor_control.right_motor_control_pid.f_Calculate(&(motor_control.right_motor_control_pid),motor_control.right_current_vel,motor_control.right_target_vel);
   motor_control.right_motor_output = motor_control.right_motor_control_pid.output;
   */
   int L_Value, R_Value;
   
   L_Value = (int)(motor_control.left_motor_output);
   R_Value = (int)(motor_control.right_motor_output);

   if (L_Value >= 0)
   {
      FTM_PWM_Duty(FTM0, FTM_CH2, 0);
      FTM_PWM_Duty(FTM0, FTM_CH0, L_Value);
   }
   else
   {
      FTM_PWM_Duty(FTM0, FTM_CH2, -L_Value);
      FTM_PWM_Duty(FTM0, FTM_CH0, 0);
   }
   if (R_Value >= 0)
   {
      FTM_PWM_Duty(FTM0, FTM_CH3, 0);
      FTM_PWM_Duty(FTM0, FTM_CH1, R_Value);
   }
   else
   {
      FTM_PWM_Duty(FTM0, FTM_CH3, -R_Value);
      FTM_PWM_Duty(FTM0, FTM_CH1, 0);
   }
}

void vision_out()
{
  if(vision_control.flag)
  {
    vision_control.delayTime += 1;
    if (vision_control.delayTime>=10000) // 延时时间
    {
      vision_control.flag = 0;
      vision_control.delayTime = -1;
    }
  }else{
      CCD_Capture();//CCD采集图像
      CCD1_Normalization();
      for(int i=60;i<120;i++)
    {
      if(CCD_Buff1[i]<80)
      {
        right=i;
        break;
      }
    }
    for(int j=60;j>6;j--)
    {
      if(CCD_Buff1[j]<80)
      {
        left=j;
        break;
      }
    }
    
    vision_control.dleft=vision_control.left1-vision_control.left;
    vision_control.dright=vision_control.right1-vision_control.right;
 vision_control.vision_control_output=vision_control.left+vision_control.right-120;//输出角速度
      if(vision_control.vision_control_output<-10&&
         vision_control.dright>20&&
         vision_control.delayTime==0)//右边边缘值以及右边缘变化值突变需要调参
      {
        vision_control.flag+=1;//输出flag，当其等于1时，进行一次进入环岛结束时需要将flag+1
      }
    }
   vision_control.left1= vision_control.left;
   vision_control.right1= vision_control.right;
}